#ifndef __WIZ_GEOMETRY_EULERANGLES_HPP__SHANHAOBO_19800429__
#define __WIZ_GEOMETRY_EULERANGLES_HPP__SHANHAOBO_19800429__

#include "../../WizAlgebra/Include/Linear/Vector3/WizAlgebraVector3.hpp"
#include "../../WizAlgebra/Include/Linear/Matrix33/WizAlgebraMatrix33.hpp"

namespace Wiz
{
    namespace EulerAngles
    {
        template<class ElementT = ::Wiz::Real::Type>
        struct Type : public ::Wiz::Vector3::Type<ElementT>
        {
            typedef ::Wiz::Vector3::Type<tElement>                  tSuper;
            typedef ::Wiz::Vector3::Type<tElement>                  tCore;
            typedef ::Wiz::Matrix33::RowMajor::Type<tElement>       tRowMajorMatrix33;
            typedef ::Wiz::Matrix33::ColMajor::Type<tElement>       tColMajorMatrix33;
            ////////////////////////////////////////////////////////////////
            Type()
            {
            }
            Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
            {
            }
            Type(const tSuper& Spr) : tSuper(Spr)
            {
            }
            WIZ_INLINE tThis& operator=(const tSuper& Spr)
            {
                this->x = Spr.x;
                this->y = Spr.y;
                this->z = Spr.z;

                return *this;
            }
            ////////////////////////////////////////////////////////////////
        };

        namespace XYZ
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;

                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                /// Calculate By RotMat_X * RotMat_Y * RotMat_Z
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ;
                    Result.e01 = -CosY * SinZ;
                    Result.e02 =  SinY;

                    Result.e10 =  SinX * SinY * CosZ + CosX * SinZ;
                    Result.e11 = -SinX * SinY * SinZ + CosX * CosZ;
                    Result.e12 = -SinX * CosY;

                    Result.e20 = -CosX * SinY * CosZ + SinX * SinZ;
                    Result.e21 =  CosX * SinY * SinZ + SinX * CosZ;
                    Result.e22 =  CosX * CosY;
                }
                /// Calculate according to above algorithms
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& InMat)
                {
                    const tElement AngY     = ::Wiz::ASin(InMat.e02);
                    if (::Wiz::IsEqual(::Wiz::Abs(AngY), ::Wiz::PIHalf))
                    {
                        
                    }
                    const tElement InvCosY  = 1 / ::Wiz::Cos(AngY);
                    const tElement CosX     = InMat.e22 * InvCosY;
                    const tElement AngX     = ::Wiz::ACos(CosX);
                    const tElement SinZ     = - InMat.e01 * InvCosY;
                    const tElement AngZ     = ::Wiz::ASin(SinZ);

                    this->x = AngX;
                    this->y = AngY;
                    this->z = AngZ;
                }

                ///////////////////////////////////////////////////////////////
                /// Calculate By RotMat_X * RotMat_Y * RotMat_Z
                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ;
                    Result.e10 =  SinX * SinY * CosZ + CosX * SinZ;
                    Result.e20 = -CosX * SinY * CosZ + SinX * SinZ;

                    Result.e01 = -CosY * SinZ;
                    Result.e11 = -SinX * SinY * SinZ + CosX * CosZ;
                    Result.e21 =  CosX * SinY * SinZ + SinX * CosZ;

                    Result.e02 =  SinY;
                    Result.e12 = -SinX * CosY;
                    Result.e22 =  CosX * CosY;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& InMat)
                {
                    const tElement AngY     = ::Wiz::ASin(InMat.e02);
                    const tElement InvCosY  = 1 / ::Wiz::Cos(AngY);
                    const tElement CosX     = InMat.e22 * InvCosY;
                    const tElement AngX     = ::Wiz::ACos(CosX);
                    const tElement SinZ     = - InMat.e01 * InvCosY;
                    const tElement AngZ     = ::Wiz::ASin(SinZ);

                    this->x = AngX;
                    this->y = AngY;
                    this->z = AngZ;
                }
            };
        } /// end of namespace XYZ

        namespace XZY
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                ////////////////////////////////////////////////////////////////
                typedef ::Wiz::Matrix33::RowMajor::Type<ElementT>       tMatrix33;
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;
                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY;
                    Result.e01 = -SinZ;
                    Result.e02 =  CosZ * SinY;

                    Result.e10 =  CosX * SinZ * CosY + SinX * SinY;
                    Result.e11 =  CosX * CosZ;
                    Result.e12 =  CosX * SinZ * SinY - SinX * CosY;

                    Result.e20 =  SinX * SinZ * CosY - CosX * SinY;
                    Result.e21 =  SinX * CosZ;
                    Result.e22 =  SinX * SinZ * SinY + CosX * CosY;
                }
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& InMat)
                {
                    const tElement AngZ = - ::Wiz::ASin(InMat.e01);
                    const tElement InvCosZ = 1 / ::Wiz::Cos(AngZ);
                    const tElement CosY = InMat.e00 * InvCosZ;
                    const tElement AngY = ::Wiz::ACos(CosY);
                    const tElement CosX = InMat.e11 * InvCosZ;
                    const tElement AngX = ::Wiz::ACos(CosX);

                    this->x = AngX;
                    this->y = AngY;
                    this->z = AngZ;
                }

                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY;
                    Result.e10 =  CosX * SinZ * CosY + SinX * SinY;
                    Result.e20 =  SinX * SinZ * CosY - CosX * SinY;

                    Result.e01 = -SinZ;
                    Result.e11 =  CosX * CosZ;
                    Result.e21 =  SinX * CosZ;

                    Result.e02 =  CosZ * SinY;
                    Result.e12 =  CosX * SinZ * SinY - SinX * CosY;
                    Result.e22 =  SinX * SinZ * SinY + CosX * CosY;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& InMat)
                {
                    const tElement AngZ = - ::Wiz::ASin(InMat.e01);
                    const tElement InvCosZ = 1 / ::Wiz::Cos(AngZ);
                    const tElement CosY = InMat.e00 * InvCosZ;
                    const tElement AngY = ::Wiz::ACos(CosY);
                    const tElement CosX = InMat.e11 * InvCosZ;
                    const tElement AngX = ::Wiz::ACos(CosX);

                    this->x = AngX;
                    this->y = AngY;
                    this->z = AngZ;
                }
            };
        } /// end of namespace XZY

        namespace YXZ
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                ////////////////////////////////////////////////////////////////
                typedef ::Wiz::Matrix33::RowMajor::Type<ElementT>       tMatrix33;
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;
                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ + SinY * SinX * SinZ;
                    Result.e01 = -CosY * SinZ + SinY * SinX * CosZ;
                    Result.e02 =  SinY * CosX;

                    Result.e10 =  CosX * SinZ;
                    Result.e11 =  CosX * CosZ;
                    Result.e12 = -SinX;

                    Result.e20 = -SinY * CosZ + CosY * SinX * SinZ;
                    Result.e21 =  SinY * SinZ + CosY * SinX * CosZ;
                    Result.e22 =  CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& Result)
                {
                }


                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ + SinY * SinX * SinZ;
                    Result.e10 =  CosX * SinZ;
                    Result.e20 = -SinY * CosZ + CosY * SinX * SinZ;

                    Result.e01 = -CosY * SinZ + SinY * SinX * CosZ;
                    Result.e11 =  CosX * CosZ;
                    Result.e21 =  SinY * SinZ + CosY * SinX * CosZ;

                    Result.e02 =  SinY * CosX;
                    Result.e12 = -SinX;
                    Result.e22 =  CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& Result)
                {
                }
            };
        } /// end of namespace YXZ

        namespace YZX
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                ////////////////////////////////////////////////////////////////
                typedef ::Wiz::Matrix33::RowMajor::Type<ElementT>       tMatrix33;
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;
                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ;
                    Result.e01 = -CosY * SinZ * CosX + SinY * SinX;
                    Result.e02 =  CosY * SinZ * SinX + SinY * CosX;

                    Result.e10 =  SinZ;
                    Result.e11 =  CosZ * CosX;
                    Result.e12 = -CosZ * SinX;

                    Result.e20 = -SinY * CosZ;
                    Result.e21 =  SinY * SinZ * CosX + CosY * SinX;
                    Result.e22 = -SinY * SinZ * SinX + CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& Result)
                {
                }


                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosY * CosZ;
                    Result.e10 =  SinZ;
                    Result.e20 = -SinY * CosZ;

                    Result.e01 = -CosY * SinZ * CosX + SinY * SinX;
                    Result.e11 =  CosZ * CosX;
                    Result.e21 =  SinY * SinZ * CosX + CosY * SinX;

                    Result.e02 =  CosY * SinZ * SinX + SinY * CosX;
                    Result.e12 = -CosZ * SinX;
                    Result.e22 = -SinY * SinZ * SinX + CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& Result)
                {
                }
            };
        } /// end of namespace YZX

        namespace ZXY
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                ////////////////////////////////////////////////////////////////
                typedef ::Wiz::Matrix33::RowMajor::Type<ElementT>       tMatrix33;
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;
                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY - SinZ * SinX * SinY;
                    Result.e01 = -SinZ * CosX;
                    Result.e02 =  CosZ * SinY + SinZ * SinX * CosY;

                    Result.e10 =  SinZ * CosY + CosZ * SinX * SinY;
                    Result.e11 =  CosZ * CosX;
                    Result.e12 =  SinZ * SinY - CosZ * SinX * CosY;

                    Result.e20 = -CosX * SinY;
                    Result.e21 =  SinX;
                    Result.e22 =  CosX * CosY;
                }
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& Result)
                {
                }


                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY - SinZ * SinX * SinY;
                    Result.e10 =  SinZ * CosY + CosZ * SinX * SinY;
                    Result.e10 =  SinZ * CosY + CosZ * SinX * SinY;

                    Result.e01 = -SinZ * CosX;
                    Result.e11 =  CosZ * CosX;
                    Result.e21 =  SinX;

                    Result.e02 =  CosZ * SinY + SinZ * SinX * CosY;
                    Result.e12 =  SinZ * SinY - CosZ * SinX * CosY;
                    Result.e22 =  CosX * CosY;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& Result)
                {
                }
            };
        } /// end of namespace ZXY

        namespace ZYX
        {
            template<class ElementT = ::Wiz::Real::Type>
            struct Type : public ::Wiz::EulerAngles::Type<ElementT>
            {
                ////////////////////////////////////////////////////////////////
                typedef ::Wiz::Matrix33::RowMajor::Type<ElementT>       tMatrix33;
                typedef ::Wiz::EulerAngles::Type<ElementT>    tSuper;
                ////////////////////////////////////////////////////////////////
                Type()
                {
                }
                Type(const tElement& Ex, const tElement& Ey, const tElement& Ez) : tSuper(Ex, Ey, Ez)
                {
                }
                Type(const tCore& Spr) : tSuper(Spr)
                {
                }
                WIZ_INLINE tThis& operator=(const tCore& Cr)
                {
                    this->x = Cr.x;
                    this->y = Cr.y;
                    this->z = Cr.z;

                    return *this;
                }
                ////////////////////////////////////////////////////////////////
                ::Wiz::Void::Type ToMatrix(tRowMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY;
                    Result.e01 = -SinZ * CosX + CosZ * SinY * SinX;
                    Result.e02 =  SinZ * SinX + CosZ * SinY * CosX;

                    Result.e10 =  SinZ * CosY;
                    Result.e11 =  CosZ * CosX + SinZ * SinY * SinX;
                    Result.e12 = -CosZ * SinX + SinZ * SinY * CosX;

                    Result.e20 = -SinY;
                    Result.e21 =  CosY * SinX;
                    Result.e22 =  CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tRowMajorMatrix33& Result)
                {
                }


                ::Wiz::Void::Type ToMatrix(tColMajorMatrix33& Result) const
                {
                    tElement SinX, CosX, SinY, CosY, SinZ, CosZ;
                    ::Wiz::SinCosFast(SinX, CosX, this->x);
                    ::Wiz::SinCosFast(SinY, CosY, this->y);
                    ::Wiz::SinCosFast(SinZ, CosZ, this->z);

                    Result.e00 =  CosZ * CosY;
                    Result.e10 =  SinZ * CosY;
                    Result.e20 = -SinY;

                    Result.e01 = -SinZ * CosX + CosZ * SinY * SinX;
                    Result.e11 =  CosZ * CosX + SinZ * SinY * SinX;
                    Result.e21 =  CosY * SinX;

                    Result.e02 =  SinZ * SinX + CosZ * SinY * CosX;
                    Result.e12 = -CosZ * SinX + SinZ * SinY * CosX;
                    Result.e22 =  CosY * CosX;
                }
                ::Wiz::Void::Type FromMatrix(const tColMajorMatrix33& Result)
                {
                }
            };
        } /// end of namespace ZYX
    } /// end of namespace EulerAngles
} /// end of namespace Wiz

#endif /*__WIZ_GEOMETRY_EULERANGLES_HPP__SHANHAOBO_19800429__*/
